#include "Arduino_BMI270_BMM150.h"

const float THRESHOLD = 1.6;
const int SAMPLELOOPTIME_TIMES = 500;
const int LOGICTICK_MS = 1000 / 100;

bool sampleBeginning = false;
int lastLogicTick = 0;
int beginSampleTime = 0;
int sampleNum = 0;
float aX_t, aY_t, aZ_t = 0.0;
float aX,aY,aZ,gX,gY,gZ = 0.0;

void setup()
{
	initSerial();
	initIMU();
}

void loop()
{
	int curCpuTime_MS = millis();
	if (!sampleBeginning)
	{
		if (canSample())
		{
			sampleBeginning = true;
			beginSampleTime = curCpuTime_MS;
		}
	}
	else
	{
		if ((curCpuTime_MS - lastLogicTick) >= LOGICTICK_MS)
		{
			lastLogicTick = curCpuTime_MS;
			if (sampleProcessTick()) sampleNum += 1;
		}
		if (sampleNum == SAMPLELOOPTIME_TIMES)
		{
			sampleNum = 0;
			sampleBeginning = false;
			Serial.println();
		}
	}
}

void initSerial()
{
	Serial.begin(9600);
	while (!Serial);
}

void initIMU()
{
	if (!IMU.begin())
	{
		Serial.println("Failed to initialize IMU!");
		while (1);
	}
	Serial.println("aX,aY,aZ,gX,gY,gZ");
}

bool canSample()
{
	if (IMU.accelerationAvailable())
	{
		IMU.readAcceleration(aX_t, aY_t, aZ_t);
		if ((fabs(aX_t) + fabs(aY_t) + fabs(aZ_t)) >= THRESHOLD) return true;
	}
	return false;
}

bool sampleProcessTick()
{
	if (IMU.accelerationAvailable() && IMU.gyroscopeAvailable())
	{
		IMU.readAcceleration(aX, aY, aZ);
		IMU.readGyroscope(gX, gY, gZ);
		Serial.print(aX, 3);
		Serial.print(',');
		Serial.print(aY, 3);
		Serial.print(',');
		Serial.print(aZ, 3);
		Serial.print(',');
		Serial.print(gX, 3);
		Serial.print(',');
		Serial.print(gY, 3);
		Serial.print(',');
		Serial.print(gZ, 3);		
		Serial.println();
		return true;
	}
	return false;
}

void test()
{
	IMU.readAcceleration(aX_t, aY_t, aZ_t);
	if ((fabs(aX_t) + fabs(aY_t) + fabs(aZ_t)) >= THRESHOLD)
		Serial.println((fabs(aX_t) + fabs(aY_t) + fabs(aZ_t)), 3);
	else
		Serial.println(0, 3);
}